﻿namespace ExpressIS.NetduinoDeviceLibrary {
  using System;
  using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
  

  /// <summary>
  /// MCP23017 - i2c 16 input/output port expander
  /// 
  /// Use this chip from 2.7-5.5V (good for any 3.3V or 5V setup), and you can sink/source up to 20mA from any of the I/O pins.
  /// </summary>
  /// <remarks><![CDATA[
  /// MCP23017 (N) DIP28 pin layout:
  /// 
  ///   28 27 26 25 24 23 22 21 20 19 18 17 16 15
  ///   │  │  │  │  │  │  │  │  │  │  │  │  │  │
  /// ███████████████████████████████████████████
  /// ▀██████████████████████████████████████████
  ///   █████████████████████████████████████████
  /// ▄██████████████████████████████████████████
  /// ███████████████████████████████████████████
  ///   │  │  │  │  │  │  │  │  │  │  │  │  │  │
  ///   1  2  3  4  5  6  7  8  9  10 11 12 13 14
  /// 
  /// 1   GPBO                     28 GPA7
  /// 2   GPB1                     27 GPA6
  /// 3   GPB2                     26 GPA5
  /// 4   GPB3                     25 GPA4
  /// 5   GPB4                     24 GPA3 
  /// 6   GPB5                     23 GPA2 
  /// 7   GPB6                     22 GPA1 
  /// 8   GPB7                     21 GPA0
  /// 9   VDD  (3.3v)              20 INTA
  /// 10  VSS  (GND)               19 INTB
  /// 11  NC                       18 RESET (Pullup Required)
  /// 12  SCL  (Netduino pin 4)    17 A2 (GND)
  /// 13  SDA  (Netdiono pin 5)    16 A1 (GND)
  /// 14  NC                       15 A0 (GND)
  /// 
  /// ]]></remarks>
  public sealed class MCP23017 : I2CDeviceHelper {
    private const ushort HARDWARE_BASE_ADDRESS = 0x20;

    private const byte MCP23017_IODIRA = 0x00;
    private const byte MCP23017_IPOLA = 0x02;
    private const byte MCP23017_GPINTENA = 0x04;
    private const byte MCP23017_DEFVALA = 0x06;
    private const byte MCP23017_INTCONA = 0x08;
    private const byte MCP23017_IOCONA = 0x0A;
    private const byte MCP23017_GPPUA = 0x0C;
    private const byte MCP23017_INTFA = 0x0E;
    private const byte MCP23017_INTCAPA = 0x10;
    private const byte MCP23017_GPIOA = 0x12;
    private const byte MCP23017_OLATA = 0x14;

    private const byte MCP23017_IODIRB = 0x01;
    private const byte MCP23017_IPOLB = 0x03;
    private const byte MCP23017_GPINTENB = 0x05;
    private const byte MCP23017_DEFVALB = 0x07;
    private const byte MCP23017_INTCONB = 0x09;
    private const byte MCP23017_IOCONB = 0x0B;
    private const byte MCP23017_GPPUB = 0x0D;
    private const byte MCP23017_INTFB = 0x0F;
    private const byte MCP23017_INTCAPB = 0x11;
    private const byte MCP23017_GPIOB = 0x13;
    private const byte MCP23017_OLATB = 0x15;

    private Cpu.Pin _interruptPin;
    private IMCP23017DevicePort[] _devicePorts = new IMCP23017DevicePort[16];  // There are a total of 16 i/o ports that can be created for this driver.
    private object _portCreationLock = new object();
    private InterruptPort _interruptPort = null;

    public MCP23017(I2CDevice sharedI2C, int clockRateKHz = 100, int transacitonTimeoutMs = 20, ushort hardwareAddress = HARDWARE_BASE_ADDRESS, Cpu.Pin interruptPin = Pins.GPIO_PIN_D4)
      : base(sharedI2C, hardwareAddress, clockRateKHz, transacitonTimeoutMs) 
    {
      this._interruptPin = interruptPin;
    }

    public void InitializeDevice() {
      this._sharedI2C.CreateWriteTransaction(new byte[] { }, this._configuration.Address, 1);

      this.SwapConfiguration(() =>
      {
        var result = this._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
        { 
            // Configure the device pins (BANK = 0, MIRROR = 0, SEQOP = 1, DISSLW = 0, HAEN = 1, ODR = 1, INTPOL = 0, UNUSED = 0)
            // These are actually the device Power-On-Reset values. 
            I2CDevice.CreateWriteTransaction(new byte[] { MCP23017_IOCONA, 0x2C }),
            I2CDevice.CreateWriteTransaction(new byte[] { MCP23017_IOCONB, 0x2C }),

            // Set the default operation of the pins to be digital input pins
            I2CDevice.CreateWriteTransaction(new byte[] { MCP23017_IODIRA, 0xFF }),  
            I2CDevice.CreateWriteTransaction(new byte[] { MCP23017_IODIRB, 0xFF }),  
        }, 1000);
      });
    }

    public IInputPort CreateInputPort(Cpu.Pin portId, bool glitchFilter, Port.ResistorMode resistorMode) {
      if (this._devicePorts[(int)portId] != null) {
        // This device port is already in use.
      }

      var port = new MCP23017InputPort(this, portId, glitchFilter, resistorMode);
      this._devicePorts[(int)portId] = port;

      port.ValidateAndInitialize();
      return port;
    }

    public IOutputPort CreateOutputPort(Cpu.Pin portId, bool initialState) {
      if (this._devicePorts[(int)portId] != null) {
        // This device port is already in use.
      }

      var port = new MCP23017OutputPort(this, portId, initialState);
      this._devicePorts[(int)portId] = port;

      port.ValidateAndInitialize();
      return port;
    }

    public IInterruptPort CreateInterruptPort(Cpu.Pin portId, bool glitchFilter, Port.ResistorMode resistorMode, Port.InterruptMode interruptMode) {
      lock (this._portCreationLock) {
        if (this._devicePorts[(int)portId] != null) {
          // This device port is already in use.
        }


        // Make sure our interrupt port has been created and is ready to use
        if (this._interruptPort == null) {
          this._interruptPort = new InterruptPort(this._interruptPin, true, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeBoth);
          this._interruptPort.OnInterrupt += (uint netDuinoPortId, uint value, DateTime dateTime) => {
            this.DispatchEventToChild(netDuinoPortId, value, dateTime); 
          };
        }

        var port = new MCP23017InterruptPort(this, portId, glitchFilter, resistorMode, interruptMode);
        this._devicePorts[(int)portId] = port;

        port.ValidateAndInitialize();
        return port;
      }
    }

    protected override void Dispose(bool disposing) {
      base._disposed = true;
    }

    private void ThrowIfDisposed() {
      if (this._disposed) {
        throw new ObjectDisposedException();
      }
    }

    private void DispatchEventToChild(uint portId, uint value, DateTime dateTime) {
      // Here we need to read the port information from the INT pin that caused the interrupt, and if necessary
      // have that instance rase its OnInterrupt event.

      uint portToNotify = 0;
      uint portValue = 0;

      this.SwapConfiguration(() =>
      {




      });

      MCP23017InterruptPort port = this._devicePorts[portToNotify] as MCP23017InterruptPort;
      if (port != null)  {
        port.DoOnInterrupt(portToNotify, portValue, dateTime);
      }
    }

    // This interface is only used so we have a consistent object type across all MCP23017 port instances.
    private interface IMCP23017DevicePort { }

    private sealed class MCP23017InputPort : IMCP23017DevicePort, IInputPort, IDisposable {
      private bool _disposed = false;
      private MCP23017 _owner;
      private Cpu.Pin _portId;
      private bool _glitchFilter;
      private Port.ResistorMode _resistorMode;

      private byte _gpioPin;
      private byte _gpioAddress;
      private byte _ioDirectionAddress;
      private byte _gpPullupAddress;

      public MCP23017InputPort(MCP23017 owner, Cpu.Pin portId, bool glitchFilter, Port.ResistorMode resistorMode, bool normallyClosed = true) {
        if (owner == null) {
          throw new ArgumentNullException("owner");
        }

        if (owner._disposed) {
          throw new ArgumentException("The I2C device has been disposed", "owner");
        }

        if (portId == Cpu.Pin.GPIO_NONE) {
          throw new ArgumentOutOfRangeException("portId", "The MCP23017 portId must be between GPIO_Pin0 and GPIO_Pin15.");
        }

        this._owner = owner;
        this._portId = portId;
        this._glitchFilter = glitchFilter;
        this._resistorMode = resistorMode;

        // Calcualte the internal address and pin bit position for this port.
        this._gpioPin = (byte)this._portId < 8 ? (byte)this._portId : (byte)(this._portId - 8);
        this._gpioAddress = (byte)this._portId < 8 ? MCP23017_GPIOA : MCP23017_GPIOB;
        this._ioDirectionAddress = (byte)this._portId < 8 ? MCP23017_IODIRA : MCP23017_IODIRB;
        this._gpPullupAddress = (byte)this._portId < 8 ? MCP23017_GPPUA : MCP23017_GPPUB;
      }

      public void ValidateAndInitialize() {
        this._owner.SwapConfiguration(() =>
        {
          // Get the current information for this port.  We need the information so that we don't mess with 
          // any of the other ports configured on the chip.

          // IODIR state
          byte[] ioDirState = new byte[1];
          var result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._ioDirectionAddress }),
            I2CDevice.CreateReadTransaction(ioDirState) 
          }, this._owner._transactionTimeoutMs);

          byte[] gpPullupState = new byte[1];
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpPullupAddress }),
            I2CDevice.CreateReadTransaction(gpPullupState) 
          }, this._owner._transactionTimeoutMs);


          byte[] gpiPolarity = new byte[1];
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { MCP23017_IPOLA }),
            I2CDevice.CreateReadTransaction(gpiPolarity) 
          }, this._owner._transactionTimeoutMs);



          // Make this an INPUT port
          ioDirState[0] = SetBit(ioDirState[0], this._gpioPin, true); // 0 = OUTPUT, 1 = INPUT

          // Set the pullup state for the pin
          gpPullupState[0] = SetBit(gpPullupState[0], this._gpioPin, this._resistorMode == Port.ResistorMode.PullUp);

          // Reverse the polarity for the pin
          gpiPolarity[0] = SetBit(gpiPolarity[0], this._gpioPin, true);


          // Update the configuration
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._ioDirectionAddress, ioDirState[0] }),
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpPullupAddress, gpPullupState[0] }),
            I2CDevice.CreateWriteTransaction(new byte[] { MCP23017_IPOLA, gpiPolarity[0] }),
          }, this._owner._transactionTimeoutMs);
        });
      }

      bool IInputPort.GlitchFilter {
        get {
          return this._glitchFilter;
        }
        set {
          this._glitchFilter = value;
        }
      }

      Cpu.Pin IInputPort.Id {
        get { return this._portId; }
      }

      bool IInputPort.Read() {
        this.ThrowIfDisposed();

        bool result = false;
        this._owner.SwapConfiguration(() =>
        {
          // Get the state of this port
          byte[] gpioState = new byte[1];
          var status = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress }),
            I2CDevice.CreateReadTransaction(gpioState) 
          }, this._owner._transactionTimeoutMs);

          result = GetBit(gpioState[0], this._gpioPin);
        });

        return result;
      }

      Port.ResistorMode IInputPort.Resistor {
        get {
          return this._resistorMode;
        }
        set {
          // Don't currently support changing this value after initialization
          //this._resistorMode = value;
        }
      }

      private static bool GetBit(byte value, byte bitToCheck) {
        return (value & (1 << bitToCheck)) != 0;
      }

      private static byte SetBit(byte value, byte bitToSet, bool state) {
        if (state) {
          // Set the bit ON
          return (byte)(value | (1 << bitToSet));
        }

        // Set the bit OFF (clear it)
        return (byte)(value & ~(1 << bitToSet));
      }

      private static byte ToggleBit(byte value, byte bitToToggle) {
        return (byte)(value ^ (1 << bitToToggle));
      }

      private void ThrowIfDisposed() {
        if ((this._owner == null) || (this._owner._disposed) || (this._disposed)) {
          throw new ObjectDisposedException();
        }
      }

      void IDisposable.Dispose() {
        this._disposed = true;
      }
    }

    private sealed class MCP23017OutputPort : IMCP23017DevicePort, IOutputPort, IDisposable {
      private bool _disposed = false;
      private MCP23017 _owner;
      private Cpu.Pin _portId;
      private bool _initialState;
      private byte _gpioPin;
      private byte _gpioAddress;
      private byte _ioDirectionAddress;

      public MCP23017OutputPort(MCP23017 owner, Cpu.Pin portId, bool initialState) {
        if (owner == null) {
          throw new ArgumentNullException("owner");
        }

        if (owner._disposed) {
          throw new ArgumentException("The I2C device has been disposed", "owner");
        }

        if (portId == Cpu.Pin.GPIO_NONE) {
          throw new ArgumentOutOfRangeException("portId", "The MCP23017 portId must be between GPIO_Pin0 and GPIO_Pin15.");
        }

        this._owner = owner;
        this._portId = portId;
        this._initialState = initialState;

        // Calcualte the internal address and pin bit position for this port.
        this._gpioPin = (byte)this._portId < 8 ? (byte)this._portId : (byte)(this._portId - 8);
        this._gpioAddress = (byte)this._portId < 8 ? MCP23017_GPIOA : MCP23017_GPIOB;
        this._ioDirectionAddress = (byte)this._portId < 8 ? MCP23017_IODIRA : MCP23017_IODIRB;
      }

      public void ValidateAndInitialize() {
        this._owner.SwapConfiguration(() =>
        {
          // Get the current information for this port.  We need the information so that we don't mess with 
          // any of the other ports configured on the chip.

          // GPIO state
          byte[] gpioState = new byte[1];
          var result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress }),
            I2CDevice.CreateReadTransaction(gpioState) 
          }, this._owner._transactionTimeoutMs);
          
          // IODIR state
          byte[] ioDirState = new byte[1];
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._ioDirectionAddress }),
            I2CDevice.CreateReadTransaction(ioDirState) 
          }, this._owner._transactionTimeoutMs);


          // Configure the initial state of the port
          gpioState[0] = SetBit(gpioState[0], this._gpioPin, this._initialState);

          // Make this an OUTPUT port
          ioDirState[0] = SetBit(ioDirState[0], this._gpioPin, false); // 0 = OUTPUT, 1 = INPUT

          // Update the configuration
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress, gpioState[0] }),
            I2CDevice.CreateWriteTransaction(new byte[] { this._ioDirectionAddress, ioDirState[0] }),
          }, this._owner._transactionTimeoutMs);
        });
      }

      private static bool GetBit(byte value, byte bitToCheck) {
        return (value & (1 << bitToCheck)) != 0;
      }

      private static byte SetBit(byte value, byte bitToSet, bool state) {
        if (state) {
          // Set the bit ON
          return (byte)(value | (1 << bitToSet));
        }

        // Set the bit OFF (clear it)
        return (byte)(value & ~(1 << bitToSet));
      }

      private static byte ToggleBit(byte value, byte bitToToggle) {
        return (byte)(value ^ (1 << bitToToggle));
      }

      private void ThrowIfDisposed() {
        if ((this._owner == null) || (this._owner._disposed) || (this._disposed)) {
          throw new ObjectDisposedException();
        }
      }

      Cpu.Pin IOutputPort.Id {
        get { return this._portId; }
      }

      bool IOutputPort.InitialState {
        get { return this._initialState; }
      }

      bool IOutputPort.Read() {
        this.ThrowIfDisposed();

        bool result = false;
        this._owner.SwapConfiguration(() =>
        {
          // Get the state of this port
          byte[] gpioState = new byte[1];
          var status = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress }),
            I2CDevice.CreateReadTransaction(gpioState) 
          }, this._owner._transactionTimeoutMs);

          result = GetBit(gpioState[0], this._gpioPin);
        });

        return result;
      }

      void IOutputPort.Write(bool value) {
        this.ThrowIfDisposed();

        this._owner.SwapConfiguration(() =>
        {
          // Get the state of this port
          byte[] gpioState = new byte[1];
          var status = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress }),
            I2CDevice.CreateReadTransaction(gpioState) 
          }, this._owner._transactionTimeoutMs);

          // Set the state of the pin 
          gpioState[0] = SetBit(gpioState[0], this._gpioPin, value);

          // Now update the chip
          status = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress, gpioState[0] }),
          }, this._owner._transactionTimeoutMs);
        });
      }

      void IDisposable.Dispose() {
        this._disposed = true;
      }
    }

    private sealed class MCP23017InterruptPort : IMCP23017DevicePort, IInterruptPort, IDisposable {
      private bool _disposed = false;
      private MCP23017 _owner;
      private Cpu.Pin _portId;
      private bool _glitchFilter;
      private Port.ResistorMode _resistorMode;
      private NativeEventHandler _interruptEventHandler;

      private byte _gpioPin;
      private byte _gpioAddress;
      private byte _ioDirectionAddress;
      private byte _gpPullupAddress;
      private Port.InterruptMode _interruptMode = Port.InterruptMode.InterruptNone;

      public MCP23017InterruptPort(MCP23017 owner, Cpu.Pin portId, bool glitchFilter, Port.ResistorMode resistorMode, Port.InterruptMode interruptMode) {
        if (owner == null) {
          throw new ArgumentNullException("owner");
        }

        if (owner._disposed) {
          throw new ArgumentException("The I2C device has been disposed", "owner");
        }

        if (portId == Cpu.Pin.GPIO_NONE) {
          throw new ArgumentOutOfRangeException("portId", "The MCP23017 portId must be between GPIO_Pin0 and GPIO_Pin15.");
        }

        this._owner = owner;
        this._portId = portId;
        this._glitchFilter = glitchFilter;
        this._resistorMode = resistorMode;
        this._interruptMode = interruptMode;

        // Calcualte the internal address and pin bit position for this port.
        this._gpioPin = (byte)this._portId < 8 ? (byte)this._portId : (byte)(this._portId - 8);
        this._gpioAddress = (byte)this._portId < 8 ? MCP23017_GPIOA : MCP23017_GPIOB;
        this._ioDirectionAddress = (byte)this._portId < 8 ? MCP23017_IODIRA : MCP23017_IODIRB;
        this._gpPullupAddress = (byte)this._portId < 8 ? MCP23017_GPPUA : MCP23017_GPPUB;
      }

      public void ValidateAndInitialize() {
        this._owner.SwapConfiguration(() =>
        {
          // Get the current information for this port.  We need the information so that we don't mess with 
          // any of the other ports configured on the chip.

          // IODIR state - Make this an INPUT port
          byte[] ioDirState = new byte[1];
          var result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._ioDirectionAddress }),
            I2CDevice.CreateReadTransaction(ioDirState) 
          }, this._owner._transactionTimeoutMs);
          ioDirState[0] = SetBit(ioDirState[0], this._gpioPin, true); // 0 = OUTPUT, 1 = INPUT

          // Set the pullup state for the pin
          byte[] gpPullupState = new byte[1];
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpPullupAddress }),
            I2CDevice.CreateReadTransaction(gpPullupState) 
          }, this._owner._transactionTimeoutMs);
          gpPullupState[0] = SetBit(gpPullupState[0], this._gpioPin, this._resistorMode == Port.ResistorMode.PullUp);

          // Setup the INT options.


          // Update the configuration
          result = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._ioDirectionAddress, ioDirState[0] }),
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpPullupAddress, gpPullupState[0] }),
          }, this._owner._transactionTimeoutMs);
        });
      }

      public void DoOnInterrupt(uint portId, uint state, DateTime dateTime) {
        if (!this._disposed) {
          var localEventHandler = this._interruptEventHandler;
          if (localEventHandler != null) {
            localEventHandler(portId, state, dateTime);
          }
        }
      }

      bool IInputPort.GlitchFilter {
        get {
          return this._glitchFilter;
        }
        set {
          this._glitchFilter = value;
        }
      }

      Cpu.Pin IInputPort.Id {
        get { return this._portId; }
      }

      bool IInputPort.Read() {
        this.ThrowIfDisposed();

        bool result = false;
        this._owner.SwapConfiguration(() =>
        {
          // Get the state of this port
          byte[] gpioState = new byte[1];
          var status = this._owner._sharedI2C.Execute(new I2CDevice.I2CTransaction[] 
          { 
            I2CDevice.CreateWriteTransaction(new byte[] { this._gpioAddress }),
            I2CDevice.CreateReadTransaction(gpioState) 
          }, this._owner._transactionTimeoutMs);

          result = GetBit(gpioState[0], this._gpioPin);
        });

        return result;
      }

      Port.ResistorMode IInputPort.Resistor {
        get {
          return this._resistorMode;
        }
        set {
          // Don't currently support changing this value after initialization
          //this._resistorMode = value;
        }
      }

      event NativeEventHandler IInterruptPort.OnInterrupt { 
        add { this._interruptEventHandler += value; }
        remove { this._interruptEventHandler -= value; }
      }

      private static bool GetBit(byte value, byte bitToCheck) {
        return (value & (1 << bitToCheck)) != 0;
      }

      private static byte SetBit(byte value, byte bitToSet, bool state) {
        if (state) {
          // Set the bit ON
          return (byte)(value | (1 << bitToSet));
        }

        // Set the bit OFF (clear it)
        return (byte)(value & ~(1 << bitToSet));
      }

      private static byte ToggleBit(byte value, byte bitToToggle) {
        return (byte)(value ^ (1 << bitToToggle));
      }

      private void ThrowIfDisposed() {
        if ((this._owner == null) || (this._owner._disposed) || (this._disposed)) {
          throw new ObjectDisposedException();
        }
      }

      void IDisposable.Dispose() {
        this._disposed = true;
      }
    }


  }
}
